#include "postprocess/kalman_filter/yaw_kalmanfilter.h"

YawKalmanFilter::YawKalmanFilter() {
  //x y vx vy ax ay

}

YawKalmanFilter::YawKalmanFilter(const ProcessorClass &flag) {
  processor_flag_ = flag;
}


YawKalmanFilter::~YawKalmanFilter() {}

void YawKalmanFilter::Init(int dynam_param_num, int measure_param_num,
                           int control_param_num) {
  BaseKalmanFilter::Init(dynam_param_num, measure_param_num, control_param_num);
  age_ = 0;
  P_.block<2, 2>(0, 0) *= initial_vehicle_position_noise;
  P_.block<2, 2>(2, 2) *= initial_vehicle_velocity_noise;
  P_.block<2, 2>(4, 4) *= initial_vehicle_acceleration_noise;
  H_.setIdentity();
}

void YawKalmanFilter::SetYaw(const float yaw) noexcept {
  yaw_ = yaw;
}

void YawKalmanFilter::GetYaw(float& yaw)  noexcept {
  yaw = yaw_;
}

float YawKalmanFilter::GetYaw()  noexcept {
  return yaw_;
}

void YawKalmanFilter::GetProcessorClassFlag(ProcessorClass &flag)  noexcept {
  flag = processor_flag_;
}

void YawKalmanFilter::SetProcessorClassFlag(ProcessorClass flag) noexcept {
  processor_flag_ = flag;
}

void YawKalmanFilter::SetQ(Eigen::MatrixXf& Q) {
  Q_ = Q;
}

void YawKalmanFilter::SetR(Eigen::MatrixXf& R) {
  R_ = R;
}

void YawKalmanFilter::GetState(float& yaw) noexcept {
  yaw = yaw_;
}

void YawKalmanFilter::UpdateWithObject(TrackedObjectPtr& new_object,
                                       TrackedObjectPtr const& old_object,
                                       double time_diff) {
  // kalman filter update
  update_state(new_object, old_object, time_diff);
  age_ += 1;
}

void YawKalmanFilter::UpdateWithoutObject(double time_diff) {
//  belief_center_point_ += belief_velocity_ * time_diff;
  age_ += 1;
}

void YawKalmanFilter::update_state(TrackedObjectPtr const& new_object,
                                   TrackedObjectPtr const& old_object,
                                   double time_diff) {
  //利用new_object中的processor_flag来进行判别
  // A赋值
  A_(0, 2) = time_diff;
  A_(1, 3) = time_diff;
  A_(2, 4) = time_diff;
  A_(3, 5) = time_diff;
  A_(0, 4) = time_diff * time_diff;
  A_(1, 5) = time_diff * time_diff;

  // to do
  // kalman的P、Q、R赋值
  if(processor_flag_ == ProcessorClass::Vehicle) {
    Q_(0, 0)  *= 0.05f;
    Q_(1, 1)  *= 0.05f;
    Q_(2, 2)  *= 0.08f;
    Q_(3, 3)  *= 0.30f;
    Q_(4, 4)  *= 0.02f;
    Q_(5, 5)  *= 0.05f;

    R_.setIdentity();

  } else if(processor_flag_ == ProcessorClass::Pedestrian) {

  }

  // to do
  // Assign measurement value
  Eigen::VectorXf Z = Eigen::VectorXf::Zero(dynam_param_num_);
  Z(0) = new_object->center.x();
  Z(1) = new_object->center.y();
  Z(2) = new_object->velocity.x();
  Z(3) = new_object->velocity.y();
  Z(4) = new_object->acceleration.x();
  Z(5) = new_object->acceleration.y();

  // update
  // x = A * x + B * u
  State_ = A_ * State_;
  // p = A * p * A^T + Q
  P_ = A_ * P_ * A_.transpose() + Q_;
  // K = p * H^T / (H * p * H^T + R)
  auto K = P_ * H_.transpose() * (H_ * P_ * H_.transpose() + R_);
  // x= x + K * (z - H * x)
  auto state_gain = K * (Z - H_ * State_);
  State_ = State_ + state_gain;
  // p = (I - K * H) * p
  Eigen::MatrixXf I =
    Eigen::MatrixXf::Identity(dynam_param_num_, dynam_param_num_);
  P_ = (I - K * H_) * P_;

  // assign value
//  belief_center_point_.head<2>() = State_.head<2>();
//  belief_center_point_[2] = new_object->center.z();//z值不更新，直接赋值
//  belief_velocity_.head<2>() = State_.segment(2, 2);
//  belief_velocity_(2) = 0.0f;
//  belief_acceleration_.head<2>() = State_.segment(4, 2);
//  belief_acceleration_(2) = 0.0f;
}
